//////////////
// scan function !
void scan_point(const int iLow, const int iHigh)
{
  int old_low = current_turret_position_low, old_high = current_turret_position_high;

  // bounding limit
  int iL = constrain(iLow, turret_low_min, turret_low_max);
  int iH = constrain(iHigh, turret_high_min, turret_high_max);

  send_start_trame();
  Serial.print("scan_point:");
  Serial.print("data=");
  // set position
  set_turret_position(iL, iH);
  // get distance
  short iDist = get_range_turret();
  // write it
  Serial.print(iL);
  Serial.print(" ");
  Serial.print(iH);
  Serial.print(" ");
  Serial.print(iDist);    
  send_end_trame();

  set_turret_position(old_low, old_high);
}
void scan_line(const int iLowBegin, const int iLowEnd, const int iLowStep, const int iHigh)
{
  int old_low = current_turret_position_low, old_high = current_turret_position_high;

  // bounding limit
  int iLB = constrain(iLowBegin, turret_low_min, turret_low_max);
  int iLE = constrain(iLowEnd, turret_low_min, turret_low_max);
  int iH = constrain(iHigh, turret_high_min, turret_high_max);

  set_turret_position(iLB, iH);

  send_start_trame();
  Serial.print("scan_line:");
  Serial.print("low_start="); 
  Serial.print(iLB); 
  Serial.print(",");
  Serial.print("low_end="); 
  Serial.print(iLE); 
  Serial.print(",");
  Serial.print("low_step="); 
  Serial.print(iLowStep); 
  Serial.print(",");
  Serial.print("high="); 
  Serial.print(iH); 
  Serial.print(",");
  Serial.print("data=");
  for(int iL=iLB; iL<iLE+1; iL+=iLowStep)
  {
    // set position
    set_turret_position(iL, iH);
    // get distance
    short iDist = get_range_turret();

    // write it
    if(iL!=iLB)
      Serial.print(";");
    Serial.print(iL);
    Serial.print(" ");
    Serial.print(iH);
    Serial.print(" ");
    Serial.print(iDist);
  }
  send_end_trame();  
  set_turret_position(old_low, old_high);
}
// do a line and return to start fow new one
void scan_zone_z(const int iLowBegin, const int iLowEnd, const int iLowStep, const int iHighBegin, const int iHighEnd, const int iHighStep)
{
  int old_low = current_turret_position_low, old_high = current_turret_position_high;

  // bounding limit
  int iLB = constrain(iLowBegin, turret_low_min, turret_low_max);
  int iLE = constrain(iLowEnd, turret_low_min, turret_low_max);
  int iHB = constrain(iHighBegin, turret_high_min, turret_high_max);
  int iHE = constrain(iHighEnd, turret_high_min, turret_high_max);

  set_turret_position(iLB, iHB);

  send_start_trame();
  Serial.print("scan_zone:");
  Serial.print("low_start="); 
  Serial.print(iLB); 
  Serial.print(",");
  Serial.print("low_end="); 
  Serial.print(iLE); 
  Serial.print(",");
  Serial.print("low_step="); 
  Serial.print(iLowStep); 
  Serial.print(",");
  Serial.print("high_start="); 
  Serial.print(iHB); 
  Serial.print(",");
  Serial.print("high_end="); 
  Serial.print(iHE); 
  Serial.print(",");
  Serial.print("high_step="); 
  Serial.print(iHighStep); 
  Serial.print(",");
  Serial.print("data=");

  for(int iH=iHB; iH<iHE+1; iH+=iHighStep)
  {
    for(int iL=iLB; iL<iLE+1; iL+=iLowStep)
    {
      // set position
      set_turret_position(iL, iH);

      // get distance
      short iDist = get_range_turret();

      // write it
      if(!(iH==iHB && iL==iLB))
        Serial.print(";");
      Serial.print(iL);
      Serial.print(" ");
      Serial.print(iH);
      Serial.print(" ");
      Serial.print(iDist);
    }
  }
  send_end_trame();  
  set_turret_position(old_low, old_high);
}
// do interval pair/unpair... quicker but bug some where :(
void scan_zone_s(const int iLowBegin, const int iLowEnd, const int iLowStep, const int iHighBegin, const int iHighEnd, const int iHighStep)
{
  int old_low = current_turret_position_low, old_high = current_turret_position_high;

  // bounding limit
  int iLB = constrain(iLowBegin, turret_low_min, turret_low_max);
  int iLE = constrain(iLowEnd, turret_low_min, turret_low_max);
  int iHB = constrain(iHighBegin, turret_high_min, turret_high_max);
  int iHE = constrain(iHighEnd, turret_high_min, turret_high_max);

  set_turret_position(iLB, iHB);

  send_start_trame();
  Serial.print("scan_zone:");
  Serial.print("low_start="); 
  Serial.print(iLB); 
  Serial.print(",");
  Serial.print("low_end="); 
  Serial.print(iLE); 
  Serial.print(",");
  Serial.print("low_step="); 
  Serial.print(iLowStep); 
  Serial.print(",");
  Serial.print("high_start="); 
  Serial.print(iHB); 
  Serial.print(",");
  Serial.print("high_end="); 
  Serial.print(iHE); 
  Serial.print(",");
  Serial.print("high_step="); 
  Serial.print(iHighStep); 
  Serial.print(",");
  Serial.print("data=");

  boolean leftoright = true;
  for(int iH=iHB; iH<iHE+1; iH+=iHighStep)
  {
    int lb, le, ls;

    if (leftoright)
    {
      lb = iLB;
      le = iLE+1;
      ls = iLowStep;
    }
    else
    {
      lb = iLE;
      le = iLB-1;
      ls = -iLowStep;
    }
    leftoright = !leftoright;

    for(int iL=lb; (!leftoright && iL<le) || (leftoright && iL>le); iL+=ls)
    {
      // set position
      set_turret_position(iL, iH);

      // get distance
      short iDist = get_range_turret();

      // write it
      if(!(iH==iHB && iL==iLB))
        Serial.print(";");
      Serial.print(iL);
      Serial.print(" ");
      Serial.print(iH);
      Serial.print(" ");
      Serial.print(iDist);
    }
  }
  send_end_trame();  
  set_turret_position(old_low, old_high);
}

